
#include "hal.h"
#include "rs232_interface.h"

#include <windows.h>
CRITICAL_SECTION cs; 


HANDLE DataSyncEvent; 
bool DataSync_ThreadProc_Is_Runing=false;
bool DataSync_ThreadProc_Kill_Request=false;
DWORD LastConnectionSignalingTime=0;



unsigned short measures[5];
unsigned long integrated_measures[5];
unsigned short zero_offsets_for_integrators[5];

bool SetBeep=false;
bool RequestIntegratorsReset=false;
bool UpdateRequestForIntegratorsZeroOffsets=false;

bool ChargeMeasureIsVailid=false;



struct EngineThrust
{
	char PacketType; /* musy be first */
	unsigned char YawControl;
	unsigned char AltitudeControl;
} g_EngineThrust,g_ZeroEngineThrust;




struct Beep
{
	char PacketType; /* musy be first */
	unsigned char Tone;
} g_Beep;


struct ChrMeasures
{
unsigned short BatteryVoltage_V;
unsigned short ChargerCurrent_mA;
unsigned short ChargerSupplyVoltage_V;
unsigned short AVCC_Reference_V;
unsigned short uC_Supply_V;
} ChargerMeasures;



bool EnginesIsEnable=false;

unsigned char tmp[100];


DWORD WINAPI DataSync_ThreadProc(void)
{
	DataSync_ThreadProc_Is_Runing=true;
	do
	{
		if(WaitForSingleObject( DataSyncEvent,50 )==WAIT_TIMEOUT)
		{

			tmp[0]='A';
			RS232WritePacket('t',tmp,1); /*request analog measurment*/
		
			Sleep(20);

			tmp[0]='I';
			RS232WritePacket('t',tmp,1); /*request integrators values*/

			Sleep(90);
			
		} else
		{

EnterCriticalSection(&cs);
 
		if(SetBeep)
		{
			RS232WritePacket('s',(unsigned char *)&g_Beep,sizeof(g_Beep));
			SetBeep=false;
		}

		if(RequestIntegratorsReset)
		{
			tmp[0]='R';
			RS232WritePacket('t',tmp,1); /* request integrators valuesm */
			RequestIntegratorsReset=false;
		}

		if(UpdateRequestForIntegratorsZeroOffsets)
		{
			tmp[0]='O';
			memcpy(tmp+1,zero_offsets_for_integrators,sizeof(zero_offsets_for_integrators));

			RS232WritePacket('t',tmp,11); /* Update integrators zero offsets */
			UpdateRequestForIntegratorsZeroOffsets=false;
		}


		g_EngineThrust.PacketType='E';
		if(EnginesIsEnable)
		{
			RS232WritePacket('s',(unsigned char *)&g_EngineThrust,sizeof(g_EngineThrust));
		} else
		{
			g_ZeroEngineThrust.PacketType='E';
			g_ZeroEngineThrust.YawControl=0;
			g_ZeroEngineThrust.AltitudeControl=0;
			RS232WritePacket('s',(unsigned char *)&g_ZeroEngineThrust,sizeof(g_ZeroEngineThrust));
		}

LeaveCriticalSection(&cs);
		}

	}
	while(DataSync_ThreadProc_Kill_Request==false);


	DataSync_ThreadProc_Is_Runing=false;
	return 1;
}

void WINAPI OnPacketReceived(char PacketType, unsigned char *data, unsigned char packet_len)
{
	if((PacketType=='r') && (packet_len==11) && (data[0]=='A'))
	{
		memcpy(measures,data+1,10); /* skipp packet type*/
	}
	if((PacketType=='r') && (packet_len==21) && (data[0]=='I'))
	{
		memcpy(integrated_measures,data+1,20); /* skipp packet type*/
		//wxLogDebug(_("Integ.: [%d,%d]"),integrated_measures[0],	integrated_measures[1]);
	}


	if((PacketType=='+') && (packet_len==0) )
	{
		LastConnectionSignalingTime=GetTickCount();
	}


	if((PacketType=='C') && (packet_len==sizeof(ChargerMeasures)))
	{
		memcpy(&ChargerMeasures,data,sizeof(ChargerMeasures));
		ChargeMeasureIsVailid=true;
		//wxLogDebug(_("Battery voltage: %f, current: %d"),((double)ChargerMeasures.BatteryVoltage_V)/100,ChargerMeasures.ChargerCurrent_mA);

	}

	
}



void HalTest(void)
{
	
}


void SetHelicopterThrust(double ThrustOnTailEngine_YawControl, double ThrustOnMainEngine_AltitudeControl) /*(0.0-100[%])*/
{
EnterCriticalSection(&cs);
	g_EngineThrust.YawControl=(unsigned short)(ThrustOnTailEngine_YawControl*(0xFF/100.0));
	g_EngineThrust.AltitudeControl=(unsigned short)(ThrustOnMainEngine_AltitudeControl*(0xFF/100.0));
LeaveCriticalSection(&cs);
SetEvent(DataSyncEvent);
}

void GetHelicopterThrust(double *ThrustOnTailEngine_YawControl, double *ThrustOnMainEngine_AltitudeControl) /*(0-0xff)*/
{
	*ThrustOnTailEngine_YawControl=g_EngineThrust.YawControl;
	*ThrustOnMainEngine_AltitudeControl=g_EngineThrust.AltitudeControl;

}

bool HalInit(LPCWSTR serial_port_name)
{
	LeaveCriticalSection(&cs); //sometime cs in debug is still active
	if(RS232InterfaceInit(serial_port_name,OnPacketReceived)==false) return false;
	DataSync_ThreadProc_Kill_Request=false;
	LastConnectionSignalingTime = 0;


	ChargerMeasures.BatteryVoltage_V=0;
	ChargerMeasures.ChargerCurrent_mA=0;
	ChargerMeasures.ChargerSupplyVoltage_V=0;

	DataSyncEvent = CreateEvent( 
        NULL,               // default security attributes
        FALSE,               // auto-reset event
        FALSE,              // initial state is nonsignaled
        TEXT("DataSyncEvent")  // object name
        ); 


    InitializeCriticalSection(&cs);	
   
	CreateThread( NULL, 0, (LPTHREAD_START_ROUTINE)DataSync_ThreadProc, NULL, 00, NULL);
	return true;
}

void HalDeInit(void)
{
	CloseHandle(DataSyncEvent);

	while(DataSync_ThreadProc_Is_Runing==true)
	{
		DataSync_ThreadProc_Kill_Request=true;
		Sleep(10);
	}
	DeleteCriticalSection(&cs);

	RS232InterfaceDeInit();
}




void EnablePowerForEngines(bool enable) /* true/false */
{
	EnginesIsEnable=enable;
	SetEvent(DataSyncEvent);
}


double GetHelicopterAnalogMeasure(int Channel)
{
	switch (Channel)
	{

		case ACCELX:
			return (measures[0]-512)/195.0;
		break;

		case ACCELY:
			return (measures[1]-512)/195.0;
		break;

		case ENG_CURRENT1:
			return 0;
		break;

		case ENG_CURRENT2:
			return 0;
		break;

		case BATTERY_VOLTAGE:
			return (((double)measures[4])*(2.54/1023.0))*4.5;
		break;
	}
	return 0;
}

unsigned long GetHelicopterIntegratedMeasures(int Channel)
{
	switch (Channel)
	{

		case ACCELX:
			return integrated_measures[0];
		break;

		case ACCELY:
			return integrated_measures[1];
		break;

	}
	return 0;
}


void SetLowVoltageLimit(double voltage)
{
EnterCriticalSection(&cs);


LeaveCriticalSection(&cs);
SetEvent(DataSyncEvent);
}


void ResetIntegrators(void)
{
 EnterCriticalSection(&cs);
	RequestIntegratorsReset=true;
 LeaveCriticalSection(&cs);
 SetEvent(DataSyncEvent);
}

void SetIntergatorZerosOffsets(unsigned short accelx, unsigned short accely)
{
 EnterCriticalSection(&cs);
	zero_offsets_for_integrators[0]=accelx;
	zero_offsets_for_integrators[1]=accely;
	for(int i=2;i<5;i++) zero_offsets_for_integrators[i]=0;
	UpdateRequestForIntegratorsZeroOffsets=true;
 LeaveCriticalSection(&cs);
 SetEvent(DataSyncEvent);
}



void StartBeep(unsigned char Tone)
{
	//SetIntergatorZerosOffsets(707,511);
	//ResetIntegrators(); /* for test only */

	if(Tone>10) return;
 EnterCriticalSection(&cs);
	g_Beep.PacketType='B';
	g_Beep.Tone=Tone;
	SetBeep=true;
 LeaveCriticalSection(&cs);
 SetEvent(DataSyncEvent);
}

void StopBeep(void)
{
 EnterCriticalSection(&cs);
	g_Beep.PacketType='B';
	g_Beep.Tone=0xFF;
	SetBeep=true;
 LeaveCriticalSection(&cs);
 SetEvent(DataSyncEvent);
}


DWORD GetLastConnectionSignalingTime(void)
{
	return LastConnectionSignalingTime;
}


bool GetChargerStatus(double *ChargerSupplyVoltage_V, double *BatteryVoltage_V ,double *ChargeCurrent_mA) /* return true if measures is vailid */
{

	if(ChargeMeasureIsVailid==false) return false;

	*ChargerSupplyVoltage_V=ChargerMeasures.ChargerSupplyVoltage_V/100;
	*BatteryVoltage_V=ChargerMeasures.BatteryVoltage_V/100;
	*ChargeCurrent_mA=ChargerMeasures.ChargerCurrent_mA;

	return false;
}



